#include "gps.h"

#include <math.h>

#ifndef GPS_LED_ACTIVE_HIGH
#define GPS_LED_ACTIVE_HIGH false
#endif

// ----------- \u786c\u4ef6\u7aef\u53e3\u4e0e\u8fd0\u884c\u72b6\u6001 -------------------------------------------------
static HardwareSerial GPS(1);
static int PIN_RX  = 7;
static int PIN_TX  = 6;
static int PIN_ONOFF = 5;
static int PIN_LED_GPS = -1;
static bool ONOFF_ACTIVE_HIGH = true;
static bool sLedActiveHigh = GPS_LED_ACTIVE_HIGH;

// \u8c03\u8bd5\u548c\u539f\u59cb\u8f93\u51fa\u5f00\u5173
static volatile bool sDebug = false;
static volatile bool sRawEcho = false;

// \u4e32\u53e3\u6ce2\u7279\u7387\u7ba1\u7406
static int sBaudIndex = 0;
static int sBaud = 115200;
static const int sBaudCandidates[] = {9600, 115200, 4800};
static const int sBaudCount = sizeof(sBaudCandidates) / sizeof(int);
static bool sFixedBaud = true;

// NMEA \u63a5\u6536\u65f6\u95f4\uff0c\u7528\u4e8e\u5224\u5b9a\u201c\u65b0\u9c9c\u6570\u636e\u201d
static unsigned long sLastNmeaMs = 0;

// \u4f4d\u7f6e\u4fe1\u606f
static float sLat = 0.0f;
static float sLon = 0.0f;
static String sTime = "";
static bool sHasFix = false;

// GPS LED \u72b6\u6001\u673a
enum LedMode : uint8_t {
  LED_MODE_OFF = 0,      // \u957f\u706d
  LED_MODE_SOLID,        // \u5e38\u4eae
  LED_MODE_FLASH_SLOW,   // \u6162\u95ea\uff1a\u6b63\u5728\u641c\u661f
  LED_MODE_FLASH_FAST    // \u5feb\u95ea\uff1a\u521a\u83b7\u5f97\u5b9a\u4f4d
};
static LedMode sLedMode = LED_MODE_OFF;
static bool sLedState = false;
static unsigned long sLedToggleMs = 0;
static bool sPrevFixState = false;
static unsigned long sFixChangeMs = 0;

// ----------- \u5185\u90e8\u5de5\u5177\u51fd\u6570 -------------------------------------------------------
static inline int ledLevel(bool on) {
  return ((on ? 1 : 0) ^ (sLedActiveHigh ? 0 : 1)) ? HIGH : LOW;
}

static void applyLed(bool on) {
  if (PIN_LED_GPS < 0) return;
  sLedState = on;
  digitalWrite(PIN_LED_GPS, ledLevel(on));
}

static bool nmeaChecksumOk(const String &s) {
  int star = s.indexOf('*');
  if (star < 0) return false;
  uint8_t cs = 0;
  int i = (s.length() > 0 && s[0] == '$') ? 1 : 0;
  for (; i < star; ++i) cs ^= (uint8_t)s[i];
  auto hexv = [](char c) -> int {
    if (c >= '0' && c <= '9') return c - '0';
    if (c >= 'A' && c <= 'F') return 10 + c - 'A';
    if (c >= 'a' && c <= 'f') return 10 + c - 'a';
    return -1;
  };
  if (star + 2 >= s.length()) return false;
  int v = (hexv(s[star + 1]) << 4) | hexv(s[star + 2]);
  return (v & 0xFF) == cs;
}

static String nmeaBuild(const String &payloadNoDollar) {
  String inner = payloadNoDollar;
  if (inner.length() && inner[0] == '$') inner = inner.substring(1);
  uint8_t cs = 0;
  for (size_t i = 0; i < inner.length(); ++i) cs ^= (uint8_t)inner[i];
  char hex[3];
  snprintf(hex, sizeof(hex), "%02X", cs);
  return String("$") + inner + "*" + hex + "\r\n";
}

static bool parseHexByteToken(const String &tok, uint8_t &out) {
  auto hexv = [](char c) -> int {
    if (c >= '0' && c <= '9') return c - '0';
    if (c >= 'A' && c <= 'F') return 10 + c - 'A';
    if (c >= 'a' && c <= 'f') return 10 + c - 'a';
    return -1;
  };
  if (tok.length() < 1 || tok.length() > 2) return false;
  int v = 0;
  for (size_t i = 0; i < tok.length(); ++i) {
    int n = hexv(tok[i]);
    if (n < 0) return false;
    v = (v << 4) | n;
  }
  out = (uint8_t)v;
  return true;
}

static void setBaudByIndex(int idx) {
  if (idx < 0) idx = 0;
  if (idx >= sBaudCount) idx = 0;
  sBaudIndex = idx;
  sBaud = sBaudCandidates[sBaudIndex];
  GPS.end();
  GPS.begin(sBaud, SERIAL_8N1, PIN_RX, PIN_TX);
  Serial.printf("[GPS] \u5207\u6362\u6ce2\u7279\u7387 %d bps (\u5019\u9009 %d)\n", sBaud, sBaudIndex);
}

static void updateGpsLed() {
  if (PIN_LED_GPS < 0) return;
  unsigned long now = millis();
  bool fresh = (now - sLastNmeaMs) < 5000UL;

  if (sHasFix != sPrevFixState) {
    sPrevFixState = sHasFix;
    sFixChangeMs = now;
  }

  LedMode target;
  if (!fresh) {
    target = LED_MODE_OFF;
  } else if (sHasFix) {
    target = (now - sFixChangeMs < 10000UL) ? LED_MODE_FLASH_FAST : LED_MODE_SOLID;
  } else {
    target = LED_MODE_FLASH_SLOW;
  }

  if (target != sLedMode) {
    sLedMode = target;
    sLedToggleMs = now;
    switch (sLedMode) {
      case LED_MODE_OFF:
        applyLed(false);
        break;
      case LED_MODE_SOLID:
        applyLed(true);
        break;
      case LED_MODE_FLASH_SLOW:
      case LED_MODE_FLASH_FAST:
        applyLed(true);
        break;
    }
  }

  if (sLedMode == LED_MODE_FLASH_SLOW || sLedMode == LED_MODE_FLASH_FAST) {
    unsigned long interval = (sLedMode == LED_MODE_FLASH_FAST) ? 150UL : 700UL;
    if (now - sLedToggleMs >= interval) {
      sLedToggleMs = now;
      sLedState = !sLedState;
      applyLed(sLedState);
    }
  }
}

// ----------- NMEA \u89e3\u6790 ---------------------------------------------------------
static void parseRMC(String rmc) {
  int idx = 0;
  String parts[20];
  while (rmc.length() > 0 && idx < 20) {
    int comma = rmc.indexOf(',');
    if (comma == -1) { parts[idx++] = rmc; break; }
    parts[idx++] = rmc.substring(0, comma);
    rmc = rmc.substring(comma + 1);
  }
  if (parts[2] == "A") {
    sTime = parts[1];
    auto decodeLat = [](const String &v, const String &hemi) {
      float raw = v.toFloat();
      int deg = int(raw / 100);
      float mins = raw - deg * 100.0f;
      float out = deg + mins / 60.0f;
      if (hemi == "S") out = -out;
      return out;
    };
    auto decodeLon = [](const String &v, const String &hemi) {
      float raw = v.toFloat();
      int deg = int(raw / 100);
      float mins = raw - deg * 100.0f;
      float out = deg + mins / 60.0f;
      if (hemi == "W") out = -out;
      return out;
    };
    sLat = decodeLat(parts[3], parts[4]);
    sLon = decodeLon(parts[5], parts[6]);
    sHasFix = true;
    if (sDebug) {
      Serial.printf("[GPS/RMC] fix OK lat=%.5f lon=%.5f utc=%s\n",
                    sLat, sLon, sTime.c_str());
    }
  } else {
    if (sDebug) Serial.println("[GPS/RMC] status invalid");
    sHasFix = false;
  }
}

static void parseGGA(String gga) {
  if (!nmeaChecksumOk(gga)) { if (sDebug) Serial.println("[GPS/GGA] checksum BAD"); return; }
  String s = gga;
  String p[20];
  int idx = 0;
  while (s.length() && idx < 20) {
    int c = s.indexOf(',');
    if (c < 0) { p[idx++] = s; break; }
    p[idx++] = s.substring(0, c);
    s = s.substring(c + 1);
  }
  auto toLat = [](const String &v, const String &hemi) {
    float raw = v.toFloat();
    int deg = int(raw / 100);
    float mins = raw - deg * 100.0f;
    float out = deg + mins / 60.0f;
    if (hemi == "S") out = -out;
    return out;
  };
  auto toLon = [](const String &v, const String &hemi) {
    float raw = v.toFloat();
    int deg = int(raw / 100);
    float mins = raw - deg * 100.0f;
    float out = deg + mins / 60.0f;
    if (hemi == "W") out = -out;
    return out;
  };
  int fixQ = p[6].toInt();
  if (fixQ > 0) {
    sLat = toLat(p[2], p[3]);
    sLon = toLon(p[4], p[5]);
    sHasFix = true;
  }
  if (sDebug) {
    const char* qdesc =
      (fixQ==0?"invalid":fixQ==1?"GPS":fixQ==2?"DGPS":fixQ==3?"PPS":
       fixQ==4?"RTK-FIX":fixQ==5?"RTK-FLOAT":fixQ==6?"DEAD-RECKON":
       fixQ==7?"MANUAL":fixQ==8?"SIM":"?");
    Serial.printf("[GPS/GGA] fix=%d(%s) lat=%.5f lon=%.5f\n",
                  fixQ, qdesc, sLat, sLon);
  }
}

static void parseGSA(String gsa) {
  if (!nmeaChecksumOk(gsa)) { if (sDebug) Serial.println("[GPS/GSA] checksum BAD"); return; }
  if (!sDebug) return;
  String s = gsa;
  String p[22];
  int idx = 0;
  while (s.length() && idx < 22) {
    int c = s.indexOf(',');
    if (c < 0) { p[idx++] = s; break; }
    p[idx++] = s.substring(0, c);
    s = s.substring(c + 1);
  }
  int fixType = p[2].toInt();
  float pdop = p[15].toFloat();
  float hdop = p[16].toFloat();
  float vdop = p[17].toFloat();
  const char* fdesc = (fixType==1?"no-fix":fixType==2?"2D":fixType==3?"3D":"?");
  Serial.printf("[GPS/GSA] fix=%d(%s) PDOP=%.1f HDOP=%.1f VDOP=%.1f\n",
                fixType, fdesc, pdop, hdop, vdop);
}

static void parseGSV(String gsv) {
  if (!nmeaChecksumOk(gsv)) { if (sDebug) Serial.println("[GPS/GSV] checksum BAD"); return; }
  if (!sDebug) return;
  String s = gsv;
  String p[32];
  int idx = 0;
  while (s.length() && idx < 32) {
    int c = s.indexOf(',');
    if (c < 0) { p[idx++] = s; break; }
    p[idx++] = s.substring(0, c);
    s = s.substring(c + 1);
  }
  int totalMsgs = p[1].toInt();
  int msgNum = p[2].toInt();
  int totalSats = p[3].toInt();
  Serial.printf("[GPS/GSV] msg %d/%d, total sats=%d\n", msgNum, totalMsgs, totalSats);
}

static void parseVTG(String vtg) {
  if (!nmeaChecksumOk(vtg)) { if (sDebug) Serial.println("[GPS/VTG] checksum BAD"); return; }
  if (!sDebug) return;
  String s = vtg;
  String p[16];
  int idx = 0;
  while (s.length() && idx < 16) {
    int c = s.indexOf(',');
    if (c < 0) { p[idx++] = s; break; }
    p[idx++] = s.substring(0, c);
    s = s.substring(c + 1);
  }
  float cog = p[1].toFloat();
  float spd = p[7].toFloat();
  Serial.printf("[GPS/VTG] course=%.1f deg, speed=%.1f km/h\n", cog, spd);
}

// ----------- \u516c\u5171 API ----------------------------------------------------------
void gpsInit(int rxPin, int txPin, int onoffPin, int ledPin, bool onoffActiveHigh, int defaultBaud) {
  PIN_RX = rxPin;
  PIN_TX = txPin;
  PIN_ONOFF = onoffPin;
  PIN_LED_GPS = ledPin;
  ONOFF_ACTIVE_HIGH = onoffActiveHigh;
  sLedActiveHigh = GPS_LED_ACTIVE_HIGH;
  if (PIN_LED_GPS >= 0) {
    pinMode(PIN_LED_GPS, OUTPUT);
    applyLed(false);
    sLedMode = LED_MODE_OFF;
    sLedToggleMs = 0;
  }
  sPrevFixState = false;
  sFixChangeMs = millis();

  pinMode(PIN_ONOFF, OUTPUT);
  gpsPowerOn();
  Serial.printf("[GPS] ONOFF pin set %s (active)\n", ONOFF_ACTIVE_HIGH ? "HIGH" : "LOW");
  delay(100);

  sBaud = defaultBaud;
  sFixedBaud = true;
  GPS.begin(sBaud, SERIAL_8N1, PIN_RX, PIN_TX);
  Serial.printf("[GPS] UART started @ %d bps (RX=%d TX=%d)\n", sBaud, PIN_RX, PIN_TX);
}

void gpsHandle() {
  static String line;
  static bool inSentence = false;
  while (GPS.available()) {
    char c = GPS.read();
    if (sRawEcho) {
      if (c == '\r') {
      } else if (c == '\n') {
        Serial.println();
      } else if (c >= 0x20 && c <= 0x7E) {
        Serial.write(c);
      } else {
        Serial.printf("<%02X>", (uint8_t)c);
      }
    }
    sLastNmeaMs = millis();
    if (!inSentence) {
      if (c == '$') { inSentence = true; line = "$"; }
      continue;
    }
    if (c == '\r') continue;
    if (c == '\n') {
      if (line.length() > 0) {
        if (line.startsWith("$BDRMC") || line.startsWith("$GNRMC") || line.startsWith("$GPRMC")) parseRMC(line);
        else if (line.startsWith("$GPGGA") || line.startsWith("$GNGGA") || line.startsWith("$BDGGA") || line.startsWith("$GBGGA")) parseGGA(line);
        else if (line.startsWith("$GPGSA") || line.startsWith("$GNGSA") || line.startsWith("$BDGSA") || line.startsWith("$GBGSA")) parseGSA(line);
        else if (line.startsWith("$GPGSV") || line.startsWith("$GNGSV") || line.startsWith("$BDGSV") || line.startsWith("$GBGSV")) parseGSV(line);
        else if (line.startsWith("$GPVTG") || line.startsWith("$GNVTG") || line.startsWith("$BDVTG") || line.startsWith("$GBVTG")) parseVTG(line);
        else if (sDebug) {
          Serial.printf("[GPS/NMEA] %s\n", line.c_str());
        }
      }
      line = "";
      inSentence = false;
      continue;
    }
    if (line.length() > 160) {
      line = "";
      inSentence = false;
      continue;
    }
    if ((c >= 0x20 && c <= 0x7E) || c == '*' || c == ',')
      line += c;
  }

  if (!sFixedBaud && millis() - sLastNmeaMs > 5000UL) {
    gpsScanBaud();
    sLastNmeaMs = millis();
  }

  updateGpsLed();
}

void gpsPowerOn() {
  digitalWrite(PIN_ONOFF, ONOFF_ACTIVE_HIGH ? HIGH : LOW);
}

void gpsPowerOff() {
  digitalWrite(PIN_ONOFF, ONOFF_ACTIVE_HIGH ? LOW : HIGH);
  applyLed(false);
  sLedMode = LED_MODE_OFF;
  sLedToggleMs = 0;
}

void gpsResetPulse(uint16_t ms) {
  gpsPowerOff();
  delay(ms);
  gpsPowerOn();
}

void gpsSetDebug(bool on) { sDebug = on; }
void gpsSetRaw(bool on)   { sRawEcho = on; }

void gpsSetBaud(int rate) {
  if (rate <= 0) rate = 9600;
  sBaud = rate;
  GPS.end();
  GPS.begin(sBaud, SERIAL_8N1, PIN_RX, PIN_TX);
  Serial.printf("[GPS] \u8bbe\u7f6e\u6ce2\u7279\u7387=%d\n", sBaud);
  sFixedBaud = true;
}

void gpsScanBaud() {
  if (sFixedBaud) {
    Serial.println("[GPS] \u5df2\u9501\u5b9a\u6ce2\u7279\u7387, \u8df3\u8fc7\u626b\u63cf");
    return;
  }
  setBaudByIndex((sBaudIndex + 1) % sBaudCount);
}

float gpsLatitude() { return sLat; }
float gpsLongitude() { return sLon; }
String gpsLatestTime() { return sTime; }
bool gpsHasFix() { return sHasFix && (sLat != 0.0f || sLon != 0.0f) && (millis() - sLastNmeaMs < 5000UL); }

void gpsSendRaw(const String &text) {
  String payload = text;
  if (!payload.endsWith("\n")) payload += "\r\n";
  GPS.print(payload);
}

void gpsSendNmea(const String &payloadNoDollar) {
  GPS.print(nmeaBuild(payloadNoDollar));
}

void gpsSendHexStr(const String &hexTokens) {
  String rest = hexTokens;
  rest.trim();
  if (rest.length() == 0) return;
  uint8_t buf[256];
  size_t n = 0;
  int pos = 0;
  while (pos < rest.length() && n < sizeof(buf)) {
    while (pos < rest.length() && rest[pos] == ' ') pos++;
    int start = pos;
    while (pos < rest.length() && rest[pos] != ' ') pos++;
    if (pos > start) {
      String tok = rest.substring(start, pos);
      uint8_t b;
      if (!parseHexByteToken(tok, b)) {
        Serial.printf("[GPS] \u975e\u6cd5\u5341\u516d\u8fdb\u5236\u5b57\u8282: %s\n", tok.c_str());
        n = 0;
        break;
      }
      buf[n++] = b;
    }
  }
  if (n > 0) GPS.write(buf, n);
}

void gpsSetLedActiveHigh(bool activeHigh) {
  sLedActiveHigh = activeHigh;
  applyLed(sLedState);
}

